#!/usr/bin/python2.7
# -*- coding: utf-8 -*-

import sys
import os.path
import numpy as np
from PIL import Image
# import tensorflow as tf
import rospy
from sensor_msgs.msg import PointCloud2, PointField
# 
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import Header

import time
import math 
from jsk_recognition_msgs.msg import BoundingBox, BoundingBoxArray
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import PoseStamped

def prediction_publish(pub,idx):
    global sign_mark, max_arr_marker
    start = time.clock()
    # record = np.load(os.path.join(self.npy_path, self.npy_files[idx]))
    # lidar = record[:, :, :5]

    # print("lidar", lidar.shape)
    # point cloud for SqueezeSeg segments
    header = Header()
    header.stamp = rospy.Time().now()
    header.frame_id = "ars"

    '''
    x = lidar[:, 0]
    y = lidar[:, 1]
    z = lidar[:, 2]
    i = self.inte_to_rgb(lidar[:, 3])
    cloud = np.stack((x, y, z, i))
    '''

    # x = np.array([1,2,3,4])
    # y = np.array([1,6,7,8])
    # z = np.array([1,2,3,10])
    x = np.random.randn(800)
    y = np.random.randn(800)
    z = np.random.randn(800)

    single_color = np.ones((800))
    single_color = np.random.rand(800)
    print(x,y,z,single_color)
    inte = np.stack((single_color,single_color,single_color,single_color))

    cloud = np.stack((x, y, z, single_color),1)
    print(cloud)
    # point cloud segments
    msg_segment = create_cloud_xyzil32(header, cloud)  # 点云数据量大，处理的时间就越长
    # msg_segment = pc2.create_cloud_xyz32(header, lidar[:, :3])  # took much time

    # publish
    pub.publish(msg_segment)
    # boundingbox publish
    # self.box_pub.publish()

    end = time.clock()
    rospy.loginfo("Point cloud processed bin. Took %d", idx)
    # label_file_frame = os.path.join(self.label_path, self.label_files[idx])
    
   


# create pc2_msg with 5 fields
def create_cloud_xyzil32(header, points):
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
            PointField('y', 4, PointField.FLOAT32, 1),
            PointField('z', 8, PointField.FLOAT32, 1),
            PointField('intensity', 12, PointField.FLOAT32, 1)]
    return pc2.create_cloud(header, fields, points)
if __name__ == '__main__':
    rospy.init_node('bin2rosvis_node')
    pub = rospy.Publisher("/ars_points", PointCloud2, queue_size=10)
    pos_pub = rospy.Publisher("/ars_pose",PoseStamped)


    # pose_stamped = PoseStamped()
    # pose_stamped.pose = pose
    # pose_stamped.header.frame_id = "/ars_color_frame"
    
    idx = 0    # 305
    len_files_bin = 20
    rate = rospy.Rate(10)    # 1
    while not rospy.is_shutdown():
        prediction_publish(pub,idx)
        idx += 1
        print("idx", idx)
        if idx > len_files_bin:
            idx = 0
        # time.sleep(0.2)
        rate.sleep()